#include "AMath3d.h"

Vector2 Vector2::ZERO = Vector2 (0, 0);
Vector2 Vector2::UNIT_X = Vector2 (1, 0);
Vector2 Vector2::UNIT_Y = Vector2 (0, 1);
Vector2 Vector2::NEGATIVE_UNIT_X = Vector2 (-1, 0);
Vector2 Vector2::NEGATIVE_UNIT_Y = Vector2 (0, -1);
Vector2 Vector2::UNIT_SCALE = Vector2 (1, 1);

Vector3 Vector3::ZERO = Vector3 (0, 0, 0);
Vector3 Vector3::UNIT_X = Vector3 (1, 0, 0);
Vector3 Vector3::UNIT_Y = Vector3 (0, 1, 0);
Vector3 Vector3::UNIT_Z = Vector3 (0, 0, 1);
Vector3 Vector3::NEGATIVE_UNIT_X = Vector3 (-1, 0, 0);
Vector3 Vector3::NEGATIVE_UNIT_Y = Vector3 (0, -1, 0);
Vector3 Vector3::NEGATIVE_UNIT_Z = Vector3 (0, 0, -1);
Vector3 Vector3::UNIT_SCALE = Vector3 (1, 1, 1);

float Matrix3::EPSILON = (float)(1e-06);
Matrix3 Matrix3::ZERO = Matrix3(0, 0, 0, 0, 0, 0, 0, 0, 0);
Matrix3 Matrix3::IDENTITY = Matrix3(1, 0, 0, 0, 1, 0, 0, 0, 1);

Quaternion Quaternion::ZERO = Quaternion (0.0, 0.0, 0.0, 0.0);
Quaternion Quaternion::IDENTITY = Quaternion (0.0, 0.0, 0.0, 1.0);

Matrix4 Matrix4::ZERO = Matrix4 (0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
Matrix4 Matrix4::IDENTITY = Matrix4 (1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1);

/** Gets the shortest arc quaternion to rotate this vector to the destination
    vector.
@remarks
    If you call this with a dest vector that is close to the inverse
    of this vector, we will rotate 180 degrees around the 'fallbackAxis'
	(if specified, or a generated axis if not) since in this case
	ANY axis of rotation is valid.
*/
Quaternion Vector3::getRotationTo(const Vector3& dest) const
{
	// Based on Stan Melax's article in Game Programming Gems
	Quaternion q;
	// Copy, since cannot modify local
	Vector3 v0 = *this;
	Vector3 v1 = dest;
	v0.normalize ();
	v1.normalize ();

	// NB if the crossProduct approaches zero, we get unstable because ANY axis will do
	// when v0 == -v1
	float d = v0.dotProduct (v1);
	// If dot == 1, vectors are the same
	if (d >= 1.0f - 1.0E-6)
	{
		return Quaternion::IDENTITY;
	}
	else if (d <= -1.0f + 1.0E-6)
	{
		// 180 rot! is it right?
		Vector3 v(v0.y, v0.z, v0.x);
		Vector3 vtemp = v.crossProduct(v1);
		q.FromAngleAxis(Radian(C_PI), vtemp);
		
		return q;
	}
	float s = (float)sqrt((1.0f + d) * 2.0f);
	_Assert (s != 0 && "Divide by zero!");
	float invs = 1.0f / s;
	
	Vector3 c = v0.crossProduct (v1);
	
	q.x = c.x * invs;
	q.y = c.y * invs;
	q.z = c.z * invs;
	q.w = s * 0.5f;
	
	return q;
}

